#include "GraphSearch.h"

GraphSearch::GraphSearch() {
}

GraphSearch::~GraphSearch() {
}

void GraphSearch::displaySearch(Graph & g, 
								Vector2 start, 
								Vector2 goal, 
								Grapher & gr, 
								bool penalized, 
								int maxDisplayVectors,
								bool unset,
								std::string filename,
								Vector2 x, 
								Vector2 y,
								int maxExpandVectors) {

	this->g = &g;
	gr.startNewFile(filename, x.x, x.y, y.x, y.y, maxDisplayVectors, unset);
	g.clearFlags();
	node* goalNode = g.getNode(goal);

	performSearch(g.getNode(start), goalNode, &gr, penalized, maxExpandVectors);

	std::cout << "path cost " << goalNode->cost << endl;

	std::vector<double*> rays;
	
	gr.setMaxVectors(1000);

	if(unset){
		gr.writeFrame(false);
		//gr.cmd("plot '-' with vectors lt 7 lw 3");
		gr.clearVertexBuffer();
		//gr.setUnSet(false);
		//gr.setTime(0.0);
		/*while(goalNode != NULL){
			if(goalNode->previous != NULL){
				//std::ostringstream cmdstr;
				//cmdstr << goalNode->previous->x << " " << goalNode->previous->y << " "<< goalNode->x << " " << goalNode->y;
				gr.addVector(Vector2(goalNode->previous->x, goalNode->previous->y), Vector2(goalNode->x, goalNode->y), 7, 2);
			}
			goalNode = goalNode->previous;
		}*/
		while(goalNode != NULL){
			if(goalNode->previous != NULL){
				double* tmp = new double[4];
				tmp[0] = goalNode->previous->x, tmp[1] = goalNode->previous->y, tmp[2] = goalNode->x - goalNode->previous->x, tmp[3] = goalNode->y -goalNode->previous->y;
				rays.push_back(tmp);
			}
			goalNode = goalNode->previous;
		}
		gr.WriteOutFinal(filename+".final", rays);
		//gr.cmd("e");
		//gr.writeFrame(false);
	}
	gr.closeFile();

	gr.displayAnimation();
}

node* GraphSearch::search(Graph* g, Vector2 start, Vector2 goal, bool penalized) {
	node* goalNode = g->getNode(goal);

	performSearch(g->getNode(start), goalNode, NULL, penalized);

	return goalNode;
}